#ifndef LUP_H
#define LUP_H

#include <utility>
#include <cmath>


/**
 * Reorders a sequence in-place according to a permutation.
 *
 * @param values The sequence to be reordered.
 * @param perm   Permutation that stores the zero-based index of the elements
 *      after reordering. This permutation is not altered.
 * @param n Number of elements in the sequence.
 * @remarks Both the value iterator and the index iterator must support
 *      random-access.
 * @timecomplexity <code>O(n^2)</code> operations. At most <code>(n-1)</code> 
 *      swaps.
 * @spacecomplexity Constant.
 * @ingroup Permutation
 */
template <class RanItValue, class RanItIndex>
void reorder(RanItValue &values, const RanItIndex &perm, size_t n)
{
	for (size_t i = 0; i < n - 1; ++i) // no need to place the last element
	{
		// Find the new index of the desired element.
		size_t j = perm[i];
		while (j < i)
			j = (size_t)perm[j];

		// Swap the element if not in place.
		if (j > i)
			std::swap(values[i], values[j]);
	}
}

/**
 * LUP decomposition. Given a square, non-singular matrix <code>A</code>, 
 * this procedure finds a unit lower triangular matrix <code>L</code>, 
 * an upper triangular matrix <code>U</code>, and a permutation matrix
 * <code>P</code>, such that <code>L*U = P*A</code>.
 *
 * @param A On input, contains a non-singular matrix to be decomposed.
 *      On return, if <code>A</code> is not singular, the elements 
 *      below the diagonal store the unit lower triangular matrix 
 *      <code>L</code>; the elements on and above the diagonal store 
 *      the upper triangular matrix <code>U</code>. If @c A is singular,
 *      the value is undefined.
 * @param N Number of rows (or columns) in the square matrix A.
 * @param perm A vector of length @c N that stores the permutation on 
 *      return if @c A is not singular. If @c A is singular, the result
 *      is undefined. 
 * @returns The determinant of the permutation matrix (which is either
 *      <code>1</code> or <code>-1</code>) if successful; otherwise zero.
 *
 * @timecomplexity <code>O(N^3)</code>.
 * @spacecomplexity Constant.
 * @ingroup Linear
 */
template <class value_type, class Matrix>
int lup_decompose(Matrix &A, size_t perm[], size_t N)
{
	int d = 1; // determinant of the permutation matrix

	// Initialize the permutation to identity.
	for (size_t i = 0; i < N; i++)
		perm[i] = i;

	// Perform Gaussian elimination on the k-th row of A.
	for (size_t k = 0; k < N; k++)
	{
		// Search for largest element in the k-th column as pivot.
		size_t pivot = k;
		value_type big = std::abs(A[k][k]);
		for (size_t i = k + 1; i < N; i++)
		{
			value_type t = std::abs(A[i][k]);
			if (t > big)
			{
				big = t;
				pivot = i;
			}
		}
		if (big == value_type(0))
			return 0;

		// Swap rows (of A and L) if the pivot chosen is not in the
		// current row.
		if (pivot != k)
		{
			for (size_t j = 0; j < N; j++)
			{
				std::swap(A[pivot][j], A[k][j]);
			}
			std::swap(perm[pivot], perm[k]);
			d = -d;
		}

		// Eliminate the k-th column from the remaining equations.
		for (size_t i = k + 1; i < N; i++)
		{
			A[i][k] /= A[k][k];
			value_type t = A[i][k];
			for (size_t j = k + 1; j < N; j++)
				A[i][j] -= t*A[k][j];
		}
	}
	return d;
}

/**
 * Solves a linear system given the LUP decomposition of the coefficient
 * matrix.
 *
 * @param LU The elements below the diagonal store the unit lower 
 *      triangular matrix <code>L</code>; the elements on and above
 *      the diagonal store the upper triangular matrix <code>U</code>.
 * @param N Number of rows (or columns) in the square matrix @c LU.
 * @param perm The row permutation.
 * @param B On input, stores the coefficients to solve. On output, stores
 *      the solved variables.
 *
 * @timecomplexity <code>O(N^2)</code>.
 * @spacecomplexity Constant.
 * @ingroup Linear
 */
template <class Matrix, class Value>
void lup_solve(const Matrix &LU, size_t N, const size_t perm[], Value B[])
{
	// Reorder the rows in B according to the permutation matrix.
	//euler::reorder(matrix_row_iterator<MB>(B,0), matrix_row_iterator<MB>(B,N), perm);
	reorder(B, perm, N);

	// Use forward substitution to solve Ly=b.
	// Skip leading zeros. i0 is the first non-zero row.
	size_t i0 = 0;
	for (; i0 < N && B[i0] == 0; ++i0);
	for (size_t i = i0 + 1; i < N; ++i)
	{
		for (size_t j = i0; j < i; ++j)
			B[i] -= LU[i][j]*B[j];
	}

	// Use backward substitution to solve Ux=y.
	for (size_t i = N; i > 0; )
	{
		--i;
		for (size_t j = i + 1; j < N; ++j)
			B[i] -= LU[i][j]*B[j];
		B[i] /= LU[i][i];
	}
}

#endif /* LUP_H */
